# import rospy
# from std_srvs.srv import Empty
# from sensor_msgs.msg import LaserScan

# 注释掉了ros相关依赖，便于windows下测试

import math
import numpy as np

RESOLUTION = 2*np.pi/3540

class Rplidar_Subscriber():
    INSTALL_ANGLE_BIAS = 315
    def __init__(self):
        self._scan = None

    #     rospy.Subscriber(
    #         "/scan",
    #         LaserScan,
    #         self.callback
    #     )

    # def callback(self, data: LaserScan):
    #     self._scan = data

    @property
    def ladar_data(self):
        '''get fixed scan data'''
        if(self._scan):
            return self._fix(self._scan.ranges, RESOLUTION, 315)
        else:
            return None

    def _fix(self, ranges, inc_rad, offset = INSTALL_ANGLE_BIAS):
        index = int(offset * math.pi / 180 / inc_rad)
        range_fixed = ranges[index:] + ranges[:index]
        return np.array(range_fixed)

    # def start_motor(self):
    #     rospy.wait_for_service('start_motor')
    #     s = rospy.ServiceProxy('start_motor', Empty)
    #     s.call()
    #     s.close()

    # def stop_motor(self):
    #     rospy.wait_for_service('stop_motor')
    #     s = rospy.ServiceProxy('stop_motor', Empty)
    #     s.call()
    #     s.close()


def range_filter(ranges, min = 1, max = 3, value = 0):
    ranges[ranges < min] = value
    ranges[ranges > max] = value


def angle_filter(ranges, the0, the1, value = 0):
    id0 = int(the0 / 180 * np.pi / RESOLUTION)
    id1 = int(the1 / 180 * np.pi / RESOLUTION)
    ranges[:id0] = value
    ranges[id1:] = value


# 找到杆子(最近的点)
def find_pole(ranges_valid):
    '''(angle, dist)'''
    copy = ranges_valid.copy()
    copy[copy == 0] = 16
    argmin = np.argmin(copy)

    angle = argmin * RESOLUTION
    dist = copy[argmin]
    
    return np.array([angle, dist, argmin])


def find_circle(ranges):
    max_d = np.max(ranges)
    thre = max_d / 1.5

    bin = np.zeros_like(ranges, dtype=np.uint8)
    bin[np.where(ranges > thre)] = 1

    median = bin.copy()    # 中值滤波

    length = len(bin)
    for i in range(1, length - 1):
        if bin[i - 1] == 1 and bin[i + 1] == 1:
            median[i] = 1
    
    # ---|---|------------------------|----|----
    # - l0 - r0 -------------------- l1 -- r1 --

    status = 0
    l0 = r0 = 0     # 呼啦圈两个顶点的左右边界
    l1 = r1 = 0
    for i in range(length - 1):
        if status == 0:
            if median[i] == 1:
                l0 = i
                status = 1

        elif status == 1:
            if median[i] == 1 and median[i + 1] == 0:
                r0 = i
                status = 2

        elif status == 2:
            if median[i] == 1:
                l1 = i
                status = 3

        elif status == 3:
            if median[i] == 1 and median[i + 1] == 0:
                r1 = i
                break

    if not l0 < r0 or not r0 < l1 or not l1 < r1:
        return None
    
    if l0 == 0 or r1 == len(median) - 1:
        return None
    
    if r0 - l0 < 2 or r0 - l0 > 25: # 两个边缘
        return None
    
    if r1 - l1 < 2 or r1 - l1 > 25: # 中间间距
        return None
    
    if l1 - r0 < 60:
        return None
    
    l = (l0 + r0) // 2
    r = (l1 + r1) // 2
    ld = ranges[l]
    rd = ranges[r]

    if ld == 0 or rd == 0:
       return None

    mid0 = map2xy(l, ld)
    mid1 = map2xy(r, rd)
    dist = calc_dist(mid0 ,mid1)
    
    print(l0, r0, l1, r1)
    print(dist, l, r, ld, rd)
    print()



def map2xy(index, dist):
    x = np.cos(index * RESOLUTION) * dist
    y = np.sin(index * RESOLUTION) * dist
    return (x, y)

calc_dist = lambda p0, p1: np.sqrt((p0[0] - p1[0])**2 + (p0[1] - p1[1])**2)

# slope = lambda p0, p1: 

# 将0到90度的数据，映射到300cm x 300cm的图片
def map2img(ranges, size = 300):
    res = np.zeros((size, size), dtype = np.uint8)
    
    length = len(ranges)
    for i in range(length):
        d = ranges[i]
        if d > 4 or d < 1 : continue

        x = np.cos(i * RESOLUTION) * d * 100
        y = np.sin(i * RESOLUTION) * d * 100
        res[int(x), int(y)] = 0xFF
    
    return res